#include "cf.h"

/**
 * @brief  互补滤波
 * @param  k:权重因子,参考值:0.05f
 * @retval void
 */
void cf_init(cf_t *hcf, float k)
{
    hcf->k = k;

    if (k > 1.0f) {
        hcf->k = 1.0f;
    }
    
    if (k < 0.0f) {
        hcf->k = 0.0f;
    }

    hcf->n = 0;
}

void cf_init_for_fr(cf_t *hcf, float wc, float dt)
{
    float b = 2 * MY_PI * wc * dt;

    float a = b / (1.0f + b);

    cf_init(hcf, a);
}

/**
 * @brief  互补滤波
 * @param  in:输入
 * @retval void
 */
float cf_apply(cf_t *hcf, float in)
{
    float out = in * hcf->k + hcf->n * (1.0f - hcf->k);

    hcf->n = out;

    return out;
}

/******************* (C) COPYRIGHT 2024 FUYEBING*****END OF FILE************/